#include "lscm_accelerator.h"
#define CALIBRATION_COUNT 100
#define MECHANICAL_MIN_BOUNDARY 0
#define MECHANICAL_MAX_BOUNDARY 0
#define MOVEMENT_END_COUNTER 25
#define SAMPLING_TIME        1   //millisecond
float accelerator_x_sample;
float accelerator_x_value[2];
float accelerator_x_calibration_data;
float velocity_x[2];
float position_x[2];
int32_t Movement_End_x_Count=0;

float LSCM_Accelerator_UpdateData(void)
{
	
	//Read data from sensor and update data of acceleration
	
	return accelerator_x_sample;


}

void LSCM_Accelerator_Calibration(void)
{
	float total_temp_sample=0.0f;
	
	for(int i=0;i<CALIBRATION_COUNT;i++)
	{
		total_temp_sample+=LSCM_Accelerator_UpdateData();
	
	}
  accelerator_x_calibration_data=total_temp_sample/CALIBRATION_COUNT;

}

float LSCM_Accelerator_MechanicalFilter(float sample)
{
	if((sample>=MECHANICAL_MIN_BOUNDARY)&&(sample<=MECHANICAL_MAX_BOUNDARY))
		sample=0;
	
	return sample;

}

void LSCM_Accelerator_MovementEndCheck(void)
{
	if(accelerator_x_value[1]==0)
		Movement_End_x_Count++;
	else
		Movement_End_x_Count=0;
	
	if(Movement_End_x_Count>MOVEMENT_END_COUNTER)
		velocity_x[1]=0;
	
	
}
void LSCM_Accelerator_Position(void)
{
	//Update acceleration data
	accelerator_x_value[1]=LSCM_Accelerator_UpdateData();
	//eliminating zero reference
	accelerator_x_value[1]=accelerator_x_value[1]-accelerator_x_calibration_data;
	//Remove machenical error
	accelerator_x_value[1]=LSCM_Accelerator_MechanicalFilter(accelerator_x_value[1]);
	
	LSCM_Accelerator_MovementEndCheck();
	//First integration to get velocity
	velocity_x[1]=velocity_x[0]+accelerator_x_value[0]*SAMPLING_TIME+((accelerator_x_value[1]-accelerator_x_value[0])/2)*SAMPLING_TIME;
	//Second integration to get position;
	position_x[1]=position_x[0]+velocity_x[0]*SAMPLING_TIME+((velocity_x[1]-velocity_x[0])/2)*SAMPLING_TIME;
	

	//Update the previous state of accelerator,velocity and position
	accelerator_x_value[0]=accelerator_x_value[1];
	velocity_x[0]=velocity_x[1];
	position_x[0]=position_x[1];
	
	


}

